% function ret = resetFunc(x)
% 
% n  = fix(length(x)/2);
% 
% M  = De_mat(x);
% J  = E_mat(x);
% 
% dx = [0; 0; x(n+1:end)];
% P  = [M, -J'; J, zeros(size(J, 1))] \ [M * dx; zeros(size(J, 1), 1)];
% size(P)
% R1 = [1  1  1 -1 -1;
%       0  0  0  0  1;
%       0  0  0  1  0;
%       0  0  1  0  0;
%       0  1  0  0  0];
% 
% ret = [R1 * x(1:n); R1*P(3:n+2)];
% 
% end

% degree of four
function ret = resetFunc(x)

    ndof = length(x)/2;
    dqem= [ 0; 0; x(ndof+1:end)];
%     qe = [0; 0; x(1); x(2); x(3); x(4)];
    

% size(De_mat(x))
% size(E_mat(x))

    P= dqem - inv(De_mat(x)) * E_mat(x)' * inv(E_mat(x) *...
        inv(De_mat(x)) * E_mat(x)') *E_mat(x) * dqem;    
% 
%     dx = [0; 0; x(5); x(6); x(7); x(8)];
%     M = De_mat(x);
%     J = E_mat(x);
%     P = [M, -J'; J, zeros(size(J, 1))] \ [M * dx; zeros(size(J, 1), 1)];
    
%     P
%     pause(1000);
%    R2 = [0 0 -1 -1 -1 1;
%          0 0 0 0 0 1;
%          0 0 0 0 -1 0;
%          0 0 0 1 0 0]; 

R1 = [1  1  1 -1 -1;
      0  0  0  0  1;
      0  0  0  1  0;
      0  0  1  0  0;
      0  1  0  0  0];

     
%     ret = [R1*qe; R1*P(1:6)]+[pi;0;-2*pi;zeros(5,1)];
% ret = [x(1)+x(2)-x(3)-x(4);x(4);-x(3);x(2); [0 0 1 1 -1 -1; 0 0 0 0 0 1;0
% 0 0 0 -1 0; 0 0 0 1 0 0]*P];
ret = [R1*x(1:ndof); R1*P(3:ndof+2)];    
        
end